#pragma once
#include "PrimaryGeometry.h"
#include "MatchingEstimator.h"
#include "Opencv2/Opencv.hpp"

#define KINECT_DEPTH_NOMINAL_FOCAL_LENGTH_IN_PIXELS         (571.4f)   // Based on 640x480 pixel size.
#define KINECT_DEPTH_NOMINAL_DIAGONAL_FOV                   (70.0f)
#define KINECT_DEPTH_NOMINAL_HORIZONTAL_FOV                 (58.5f)
#define KINECT_DEPTH_NOMINAL_VERTICAL_FOV                   (45.6f)

#define KINECT_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS         (531.15f)   // Based on 640x480 pixel size.
#define KINECT_COLOR_NOMINAL_INVERSE_FOCAL_LENGTH_IN_PIXELS (1.83e-3f)
#define KINECT_COLOR_NOMINAL_DIAGONAL_FOV                   ( 73.9f)
#define KINECT_COLOR_NOMINAL_HORIZONTAL_FOV                 ( 62.0f)
#define KINECT_COLOR_NOMINAL_VERTICAL_FOV                   ( 48.6f)

#define PIXEL_PER_MM										(1.4285f)
enum FingerType{
	HM_FINGER_DEFAULT,
	HM_FINGER_THUMB,
	HM_FINGER_LITTLE,
};

struct handBone
{
	//Eigen::Vector3f root_join;  // 
	Eigen::Vector3f rotation; // Vecto do nghieng theo x, y, z cua xuong do
	float length;
	float radius1;
	float radius2;

};


// Lop bieu dien ngon tay
class HmFinger
{
public:
	FingerType type;
	//handBone bone_0;
	//handBone bone_1;
	//handBone bone_2;

	handBone bone[3];
	HmFinger();
	~HmFinger();
	void setFingerType(FingerType typef);
	void setDefaultParameter();
	void setFingerParameter(int bone_index,float radious1,float radious2,float length);
	void setFingerRotation(int bone_index,Eigen::Vector3f t_rotation);
};

class HandModel
{
public:
	HandModel(void);
	~HandModel(void);

	float handScale;
	/* There are 26 DOFs on a Hand */
	Eigen::Vector3f wirst_rotation;//+3 DOFs
	Eigen::Vector3f wirst_position;//+3 DOFs
	HmFinger finger_thumb;//+4 DOFs - 2 in bone 1, 1 each other bones
	HmFinger finger_1;//+4 DOFs
	HmFinger finger_2;//+4 DOFs
	HmFinger finger_3;//+4 DOFs
	HmFinger finger_little;//+4 DOFs

	double lastTime;
	double startTime;
	unsigned int frame;

	// buffer for capture depth OPENGL
	GLfloat * buffer;

	void getHypothesis(Hypothesis h);

	void drawHandBone(handBone hm_handbone);
	void drawHandBone(handBone hm_handbone,bool MultipleDOF,bool thumb); // De ve them ngon cai 
	void drawFinger(HmFinger hm_finger);
	void drawThumb(HmFinger hm_finger);
	void drawPalm(float width, float height, float depth);

	void drawHand();
	void drawPalmAndFiveFingers();

	cv::Mat get_gl_depth(int width,int height);  // lay anh do sau
	void get_gl_depth_at(double x,double y); // lay do sau cua 1 pixel

};